/* 
 * AlignTest
*/

#include "include/mex.hpp"
#include "include/mexAdapter.hpp"
#include "AlignTest.hpp"
#include "PSINS/KFApp.h"
#include "PSINS/PSINS.h"

using namespace matlab::data;
using matlab::mex::ArgumentList;

class MexFunction : public matlab::mex::Function {
public:
    void operator()(ArgumentList outputs, ArgumentList inputs) {
        ArrayFactory Af;
        //checkArguments(outputs, inputs);

        //====================输入数据交互========================
        Array A_pos0 = Array(inputs[0]);
        Array A_imudata = Array(inputs[1]);
        size_t len = A_imudata.getNumberOfElements() / 7;
        Array avpdata = Af.createArray<double>({len,10});
        size_t len_out = avpdata.getNumberOfElements() / 10;
        //=======================================================

        //==================== 导 航 解 算 ========================
        CVect3 *pos0 = new CVect3(A_pos0[0],A_pos0[1],A_pos0[2]);
        CVect3 *vel0 = new CVect3(0.0);
        CVect3 wm, vm, att;
        double t;
        CAligni0 align = CAligni0(*pos0, *vel0);
        for (size_t i = 0; i < len; i++)
        {
            wm.i = A_imudata[i][0];
            wm.j = A_imudata[i][1];
            wm.k = A_imudata[i][2];
            vm.i = A_imudata[i][3];
            vm.j = A_imudata[i][4];
            vm.k = A_imudata[i][5];
            t = A_imudata[i][6];
            align.Update(&wm, &vm, 1, 0.01);
            att = q2att(align.qnb);
            avpdata[i][0] = att.i;
            avpdata[i][1] = att.j;
            avpdata[i][2] = att.k;
            avpdata[i][9] = t;
        }
        //=======================================================

        //====================输出数据交互========================
        outputs[0] = avpdata;
        //=======================================================
    }
};

